#!/usr/bin/env python
#
# License: BSD
#   https://raw.github.com/robotics-in-concert/rocon_multimaster/license/LICENSE
#

##############################################################################
# Imports
##############################################################################

import sys
import argparse
import re
import rospy
import rocon_gateway
import rocon_gateway_utils
import gateway_msgs.msg as gateway_msgs
import gateway_msgs.srv as gateway_srvs
import rocon_console.console as console
import rocon_python_comms

##############################################################################
# Classes
##############################################################################


class Flags(object):
    advertise = 'advertise'
    cancel = 'cancel'

##############################################################################
# Functions
##############################################################################


def parse_arguments():
    parser = argparse.ArgumentParser(description='Interactive tool for advertising local connections')
    parser.add_argument('-d', '--debug', action='store_true', help='print debugging information')
    args = parser.parse_args()
    console.debug_flag = args.debug
    return args


def resolve_flags():
    '''
      Currently only advertise/cancel flag
    '''
    response = raw_input("> Advertise or cancel advertisements (a/c): ")
    while response != 'a' and response != 'c':
        response = raw_input("> Valid options are (a/c): ")
    if response == 'a':
        return Flags.advertise
    else:
        return Flags.cancel


def resolve_public_interface(gateway_namespace):
    gateway_info = rocon_python_comms.SubscriberProxy(gateway_namespace + '/gateway_info', gateway_msgs.GatewayInfo)()
    return gateway_info.public_interface, gateway_info.public_watchlist


def resolve_unadvertise(advertise_watchlist):
    if len(advertise_watchlist) == 0:
        raise rocon_gateway.GatewayError("No advertisements to cancel, aborting.")
    index = 0
    max_index = len(advertise_watchlist) - 1
    unadvertisements = []
    if max_index == 0:
        console.debug("Automatically selecting %s (only one visible)" % advertise_watchlist[0].name)
        unadvertisements.append(advertise_watchlist[0])
    else:
        print("Select an advertisement to cancel (gateway-name-type-node):")
        for watch_rule in advertise_watchlist:
            if watch_rule.node == "None" or watch_rule.node == '':
                watch_rule.node = r'.*'
            print("    " + str(index) + ") "),
            console.pretty_print(watch_rule.name, console.green)
            print("-"),
            console.pretty_print(watch_rule.type, console.cyan)
            print("-"),
            console.pretty_print(watch_rule.node + "\n", console.yellow)
            index += 1
        input_response = raw_input("Enter [0-%s]: " % str(max_index))
        index = int(input_response)
        unadvertisements.append(advertise_watchlist[index])
    return unadvertisements


def is_in_advertised_list(connection, advertisements):
    for advertisement in advertisements:
        if advertisement.name == connection.rule.name and \
           advertisement.type == connection.rule.type and \
           advertisement.node == connection.rule.node:
            return True
    return False


def resolve_advertisements(gateway_namespace, advertisements):
    master = rocon_gateway.LocalMaster()
    connections = []
    with master.get_connection_state() as connection_dictionary:
        for connection_type in connection_dictionary:
            connections.extend(connection_dictionary[connection_type])

    connections[:] = [connection for connection in connections if not is_in_advertised_list(connection, advertisements)]
    # some things you should never advertise
    connections[:] = [connection for connection in connections
                      if not re.match('.*set_logger_level', connection.rule.name)]
    connections[:] = [connection for connection in connections if not re.match('.*get_loggers', connection.rule.name)]
    connections[:] = [connection for connection in connections if not re.match('.*rosout', connection.rule.name)]
    connections[:] = [connection for connection in connections if not re.match(
        gateway_namespace + '/.*', connection.rule.name)]
    connections[:] = [connection for connection in connections
                      if not re.match('.*zeroconf.*', connection.rule.name)]
    max_index = len(connections) - 1
    advertisement_rules = []
    if max_index == 0:
        console.warning("Automatically selecting %s (only one visible)" % connections[0].rule.name)
        advertisement_rules.append(connections[0].rule)
    elif max_index < 0:
        raise rocon_gateway.GatewayError("No advertisements possible (all are either advertised or blacklisted).")
    else:
        index = 0
        print("Select a connection to advertise (name-type-node):")
        for connection in connections:
            print("    " + str(index) + ") "),
            console.pretty_print(connection.rule.name, console.green)
            print("-"),
            console.pretty_print(connection.rule.type, console.cyan)
            print("-"),
            console.pretty_print(connection.rule.node + "\n", console.yellow)
            index += 1
        input_response = raw_input("Enter [0-%s] or regex keyed by name [e.g. /cha.* or .*]: " % str(max_index))
        try:
            index = int(input_response)
            advertisement_rules.append(connections[index].rule)
        except ValueError, e:  # It's a regex
            for connection in connections:
                if re.match(input_response, connection.rule.name):
                    advertisement_rules.append(connection.rule)
    return advertisement_rules


def advertise(gateway_namespace, rules, flag):
    advertise_service = rospy.ServiceProxy(gateway_namespace + '/advertise', gateway_srvs.Advertise)
    req = gateway_srvs.AdvertiseRequest()
    for rule in rules:
        req.cancel = True if flag == Flags.cancel else False
        if rule.node == r'.*':  # ugly hack
            rule.node = ''
        req.rules.append(rule)
        resp = advertise_service(req)

##############################################################################
# Main
##############################################################################

if __name__ == '__main__':

    rospy.init_node('advertise')
    args = parse_arguments()
    gateway_namespace = None
    flag = None
    rules = None

    try:
        gateway_namespace = rocon_gateway_utils.resolve_local_gateway()
        flag = resolve_flags()
        advertisements, advertise_watchlist = resolve_public_interface(gateway_namespace)
        if flag == Flags.cancel:
            rules = resolve_unadvertise(advertise_watchlist)
        else:
            rules = resolve_advertisements(gateway_namespace, advertisements)
    except rocon_gateway.GatewayError, e:
        console.error(str(e))
        sys.exit(1)
    console.pretty_print("Information\n", console.bold)
    print("  Local Gateway: %s" % gateway_namespace)
    print("  Operation    : %s" % flag)
    first_rule = True
    print("  Advertise Rules   : "),
    for rule in rules:
        if first_rule:
            first_rule = False
        else:
            print("               : "),
        console.pretty_print(rule.name, console.green)
        print("-"),
        console.pretty_print(rule.type, console.cyan)
        print("-"),
        console.pretty_print(rule.node + "\n", console.yellow)
    proceed = raw_input("Proceed? (y/n): ")
    if proceed == 'y':
        advertise(gateway_namespace, rules, flag)
    rospy.rostime.wallsleep(1)
